#! /usr/bin/env python

# this ROS node converts a PolygonStamped (such as might be produced by
# the color based tracking) and produces a Convex_Space.  You must pass
# the camera parameters on the command line.

import roslib
roslib.load_manifest('intersector')
import rospy

from geometry_msgs.msg import PolygonStamped
from geometry_msgs.msg import   PointStamped
from geometry_msgs.msg import   Point
from intersector.msg   import Convex_Space
from intersector.msg   import Camera2BoundingBox__setCamera
from visualization_msgs.msg import Marker

import sys   # for argv

from camera2boundingBox import Camera2BoundingBox



def input_handler(bb):
    global C2BB
    global sensor
    global algorithm
    global aux_payload

    print "input_handler(...): START: bb=%s" % bb
    print

    for i in range(0,len(bb.polygon.points)):
      for j in range(i+1,len(bb.polygon.points)):
        pt1 = bb.polygon.points[i]
        pt2 = bb.polygon.points[j]
        if pt1.x == pt2.x and pt1.y == pt2.y and pt1.z == pt2.z:
            print "PolygonStamped_2_Convex_Space.py:input_handler(): ERROR: Input polygon includes duplicate points!!! bb = %s" % bb
            return

#    print "TEST HACK"
#    bb.polygon.points[0] = Point(0,        0,        0)
#    bb.polygon.points[1] = Point(C2BB.xWid,0,        0)
#    bb.polygon.points[2] = Point(C2BB.xWid,C2BB.yHei,0)
#    bb.polygon.points[3] = Point(0,        C2BB.yHei,0)

#    save_points = bb.polygon.points
#    bb.polygon.points = (Point(C2BB.xWid/2,C2BB.yHei/2,0),
#                         Point(0,          C2BB.yHei/2,0), Point(C2BB.xWid,  C2BB.yHei/2,0),
#                         Point(C2BB.xWid/2,0,          0), Point(C2BB.xWid/2,C2BB.yHei,  0))
#    print "input_handler(...): Sending dummy points"
#    C2BB.GenPoints(bb)
#    print "input_handler(...): Restoring original points"
#    bb.polygon.points = save_points

    space = C2BB.PublishSpace(bb, sensor,algorithm, aux_payload)

    if space is None:
        print "WARNING: input_handler(): C2BB.PublishSpace() failed to return a valid space object"
        print
        return

    print "input_handler(...): space=%s" % space
    print

    global marker_pub
    global marker_color


#    marker = Marker()
#    marker.header.frame_id = "/map"
#    marker.header.stamp    = rospy.Time()
#    marker.ns = ""
#    marker.id = 0
#    marker.type   = Marker.LINE_LIST
#    marker.action = Marker.ADD
#    marker.pose.position.x = 0
#    marker.pose.position.y = 0
#    marker.pose.position.z = 0
#    marker.pose.orientation.x = 0
#    marker.pose.orientation.y = 0
#    marker.pose.orientation.z = 0
#    marker.pose.orientation.w = 0
#    marker.scale.x = .01
#    marker.scale.y = .01
#    marker.scale.z = .01
#    marker.color = marker_color
#
#    marker.points = []
#
#    for plane in space.planes:
#        # we want to find a line which passes through the origin, is
#        # perpendicular to the plane, and is only present on the "good" side
#        # of the plane.  We first try to find the point on the plane.  To do
#        # that, we start with the normal as a candidate.  We then calculate
#        # the dot product of it with itself, and compare that to d; that
#        # gives us an amount by which we must scale the normal to find a
#        # point directly on the plane.
#        candidate = Point(plane.a, plane.b, plane.c)
#        dotProd = plane.a*plane.a + plane.b*plane.b + plane.c*plane.c
#
#        scale = plane.d / dotProd
#        assert scale != 0
#
#        point1 = Point(candidate.x * scale, candidate.y * scale, candidate.z * scale)
#
#        # the 2nd point is, essentially, that 1st point plus a large multiple
#        # of the normal.  However, we must ensure that the direction of the
#        # vector is into the good side of the plane.
#        point2 = Point(point1.x + 10*1000*plane.a, point1.y + 10*1000*plane.b, point1.z + 10*1000*plane.c)
#
#        compare = point2.x * plane.a + point2.y * plane.b + point2.z * plane.c
#        assert compare != 0
#
#        if compare > 0:
#            point2 = Point(point1.x - 10*1000*plane.a, point1.y - 10*1000*plane.b, point1.z - 10*1000*plane.c)
#
#        marker.points.append(point1)
#        marker.points.append(point2)
#       
#
#    print "input_handler: publishing: marker=%s" % marker
#    marker_pub.publish(marker)



    marker = Marker()
    marker.header.frame_id = "/map"
    marker.header.stamp    = rospy.Time()
    marker.ns = ""
    marker.id = 0
    marker.type   = Marker.LINE_LIST
    marker.action = Marker.ADD
    marker.pose.position.x = 0
    marker.pose.position.y = 0
    marker.pose.position.z = 0
    marker.pose.orientation.x = 0
    marker.pose.orientation.y = 0
    marker.pose.orientation.z = 0
    marker.pose.orientation.w = 0
    marker.scale.x = .01
    marker.scale.y = .01
    marker.scale.z = .01
    marker.color = marker_color

#    marker.points = []
#    marker.points.append( Point( 0,0,0))
#    marker.points.append( Point(10,0,0))
#    marker.points.append( Point(10,0,0))
#    marker.points.append( Point(10,0,0))
#
#    print marker
#    print
#    marker_pub.publish(marker)


#    marker.points = []
#
#    point_stamped = PointStamped(bb.header, Point(0,0,0))
#    point = C2BB.tf_listener.transformPoint("/map", point_stamped)
#    marker.points.append(point.point)
#
#    point_stamped = PointStamped(bb.header, Point(5,0,0))
#    point = C2BB.tf_listener.transformPoint("/map", point_stamped)
#    marker.points.append(point.point)
#
#    marker.points.append(point.point)
#
#    point_stamped = PointStamped(bb.header, Point(0,5,0))
#    point = C2BB.tf_listener.transformPoint("/map", point_stamped)
#    marker.points.append(point.point)
#
#    marker.points.append(point.point)
#
#    point_stamped = PointStamped(bb.header, Point(0,0,5))
#    point = C2BB.tf_listener.transformPoint("/map", point_stamped)
#    marker.points.append(point.point)
#
#    print marker
#    print
#    marker_pub.publish(marker)


    marker.points = []

    points = C2BB.GenPoints(bb)

    print "input_handler: points[0] = (%f,%f,%f)" % (points[0].x, points[0].y, points[0].z)
    for i in range(1,len(points)):
        vector = Point(points[i].x - points[0].x, points[i].y - points[0].y, points[i].z - points[0].z)

        scale  = (0 - points[0].z) / vector.z
        scale = 10*1000

        points[i] = Point(points[0].x + scale*vector.x, points[0].y + scale*vector.y, points[0].z + scale*vector.z)
        print "i=%d points[i] = (%f,%f,%f)   SCALE=%f" % (i, points[i].x,points[i].y,points[i].z, scale)

    for i in range(0,len(points)):
      print points[i]

      for j in range(i+1,len(points)):
        point_stamped = PointStamped(bb.header, points[i])
        point = point_stamped # point = C2BB.tf_listener.transformPoint("/map", point_stamped)
        marker.points.append(point.point)

        point_stamped = PointStamped(bb.header, points[j])
        point = point_stamped # point = C2BB.tf_listener.transformPoint("/map", point_stamped)
        marker.points.append(point.point)

#    print marker
#    print
    marker_pub.publish(marker)



#    marker = Marker()
#    marker.header.frame_id = "/map"
#    marker.header.stamp    = rospy.Time()
#    marker.ns = ""
#    marker.id = 0
#    marker.type   = Marker.POINTS
#    marker.action = Marker.ADD
#    marker.pose.position.x = 0
#    marker.pose.position.y = 0
#    marker.pose.position.z = 0
#    marker.pose.orientation.x = 0
#    marker.pose.orientation.y = 0
#    marker.pose.orientation.z = 0
#    marker.pose.orientation.w = 0
#    marker.scale.x = .01
#    marker.scale.y = .01
#    marker.scale.z = .01
#    marker.color = marker_color
#
#    marker.points = []
#
#    for x in range(-3,3+1, 3):
#      for y in range(-3,3+1, 3):
#        marker.points.append(Point(x,y,0))
#
#    planes = space.planes + [space.planes[0]]
#    for i in range(1,len(planes)):
#        # given two planes (a1,b1,c1,d1),(a2,b2,c2,d2), they intersect
#        # the floor (z=0) at:
#        #      a1 x + b1 y + d1 = 0
#        #      a2 x + b2 y + d2 = 0
#        #
#        #         x             = (-d1 - b1 y)/ a1
#        #
#        #      a2 ((-d1 - b1 y)/a1) +    b2 y +    d2 = 0
#        #      a2  (-d1 - b1 y)     + a1 b2 y + a1 d2 = 0
#        #     -a2 d1 - a2 b1 y      + a1 b2 y + a1 d2 = 0
#        #            - a2 b1 y      + a1 b2 y         =  a2 d1 - a1 d2
#        #                    y                        = (a2 d1 - a1 d2) / (a1 b2 - a2 b1)
#
#        plane1 = planes[i-1]
#        plane2 = planes[i]
#
#        print "RUSS"
#        print "plane1=%s" % plane1
#        print "plane2=%s" % plane2
#
#        y = (plane2.a * plane1.d - plane1.a * plane2.d) / (plane1.a * plane2.b - plane2.a * plane1.b)
#        x = (-plane1.d - plane1.b * y) / plane1.a
#
#        print "(x,y)=(%f,%f)" % (x,y)
#
#        print "plane1 val=%f" % (x * plane1.a + y * plane1.b + plane1.d)
#        print "plane2 val=%f" % (x * plane2.a + y * plane2.b + plane2.d)
#        print
#
#        assert abs(x * plane1.a + y * plane1.b + plane1.d < .1)
#        assert abs(x * plane2.a + y * plane2.b + plane2.d < .1)
#
#        marker.points.append(Point(x,y,0))
#
#    print marker
#    print
#
#    marker_pub.publish(marker)
#
#    print


#    marker.points = C2BB.GenPoints(bb)
#    marker_pub.publish(marker)



if __name__ == '__main__':
    input_topic  = sys.argv[1]
    output_topic = sys.argv[2]

    xWid        =   int(sys.argv[3])
    yHei        =   int(sys.argv[4])

    upperLeft_world_x = float(sys.argv[5])
    upperLeft_world_y = float(sys.argv[6])
    upperLeft_world_z = float(sys.argv[7])

    global sensor
    global algorithm
    global aux_payload
    sensor      = sys.argv[8]
    algorithm   = sys.argv[9]
    aux_payload = sys.argv[10]



    rospy.init_node('PolygonStamped_2_Convex_Space')

    global C2BB
    C2BB = Camera2BoundingBox()

    global marker_pub

    cam_info = Camera2BoundingBox__setCamera()
    cam_info.xWid              = xWid
    cam_info.yHei              = yHei
    cam_info.upperLeft_world_x = upperLeft_world_x
    cam_info.upperLeft_world_y = upperLeft_world_y
    cam_info.upperLeft_world_z = upperLeft_world_z
    C2BB.setCamera(cam_info)
    C2BB.setPublisher(output_topic)

    marker_pub = rospy.Publisher(input_topic + "__marker", Marker)

    global marker_color
    marker_color = Marker().color
    marker_color.a = 1

    if   input_topic[-5:] == "3/red":
        marker_color.r = 1
        marker_color.g = 0
        marker_color.b = 0
    elif input_topic[-5:] == "green":
        marker_color.r = 0
        marker_color.g = 1
        marker_color.b = 0
    elif input_topic[-5:] == "white":
        marker_color.r = .5
        marker_color.g = .5
        marker_color.b = .5
    elif input_topic[-5:] == "/blue":
        marker_color.r = 0
        marker_color.g = 0
        marker_color.b = 1
    else:
        marker_color.r = 0
        marker_color.g = 0
        marker_color.b = 0

    rospy.Subscriber(input_topic, PolygonStamped, input_handler)

    rospy.spin()

